package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S6: maximizes angular resolution.
 */
public class ForceS6 extends ForceSubsystem {

	ForceS6(ForceSystem system) {
		super(system);
	}

	void applyForces(int delta) {
		for (MetroStation m1 : system.metroMap.stations) {
			if (m1.degree >= 3) {
				Body b1 = system.stationBodies[m1.id];
				Vec2 p1 = b1.getPosition();

				final double[] angles = new double[system.metroMap.stations.length];
				for (MetroStation m2 : m1.adj) {
					Body b2 = system.stationBodies[m2.id];
					Vec2 p2 = b2.getPosition();
					Vec2 p12 = p2.sub(p1);
					angles[m2.id] = Math.atan2(p12.y, p12.x);
				}

				class StationAngleComparator implements Comparator<MetroStation> {
					@Override
					public int compare(MetroStation a, MetroStation b) {
						double angleA = angles[a.id];
						double angleB = angles[b.id];
						return angleA < angleB ? -1 : angleA == angleB ? 0 : 1;
					}
				}

				List<MetroStation> list = Arrays.asList(m1.adj);
				Collections.sort(list, new StationAngleComparator());
			}
		}
	}

	@SuppressWarnings("unused")
	private void applyForces(MetroStation m1, MetroStation m2, MetroStation m3, double[] angles, double equilibrium) {
		MetroEdge e1 = system.metroMap.getEdge(m1.id, m2.id);
		MetroEdge e2 = system.metroMap.getEdge(m2.id, m3.id);

		Body b1 = system.controlPointBodies[e1.id];
		Body b2 = system.stationBodies[m2.id];
		Body b3 = system.controlPointBodies[e2.id];

		Vec2 p1 = b1.getPosition();
		Vec2 p2 = b2.getPosition();
		Vec2 p3 = b3.getPosition();

		Vec2 p21 = p1.sub(p2);
		Vec2 p23 = p3.sub(p2);

		double theta21 = Math.atan2(p21.y, p21.x); // between p21 and x-axis
		double theta23 = Math.atan2(p23.y, p23.x); // between p23 and x-axis

		// First rotate the vectors such that the vector from 2 to 1
		// lies on the x-axis. Then subtract Pi to set a straight line
		// (opposite vectors) to 0.
		double theta = (theta23 - theta21) - Math.PI;
		// Move the range to [-pi, pi).
		if (theta <= -Math.PI)
			theta += 2f * Math.PI;

		Vec2 f1 = new Vec2(-p21.y, p21.x);
		Vec2 f3 = new Vec2(p23.y, -p23.x);

		float len1 = f1.normalize();
		float len3 = f3.normalize();

		f1.mulLocal((float) theta * ForceConstants.S0_WEIGHT / len1);
		f3.mulLocal((float) theta * ForceConstants.S0_WEIGHT / len3);

		b1.applyForceToCenter(f1);
		b3.applyForceToCenter(f3);
	}
}
